
/* Copyright (c) Gong Chengming, NARI-TECH. 2011 */

#ifndef _LU_H
#define _LU_H

#include <cassert>
#include <cstddef>
#include "./matrix.h"

/* 
  An LU factorizer and solver.
*/

template<class T>
class Lu {
 public:
  typedef Square<T> MT;
  Lu() {}
  Lu(const MT& A) {
    SetA(A);
  }
  ~Lu() {}

  Lu& SetA(const MT & A) {
    _factor = A;
    Factorize(_factor);
    return *this;
  }

  template<class XT>
  const Lu& Solve(XT* x) const {
    size_t dim = _factor.Dim();

    for (size_t r = 0; r < dim; ++r) {
      for (size_t c = 0; c < r; ++c) {
        x[r] -= L(r, c) * x[c];
      }
    }

    for (size_t i = 0; i < dim; ++i) {
      size_t r = dim - i - 1;
      for (size_t c = r + 1; c < dim; ++ c) {
        x[r] -= U(r, c) * x[c];
      }
      x[r] /= U(r, r);
    }

    return *this;
  }

 private:
  MT _factor;
  const T& U(size_t i, size_t j) const {
    assert(i <= j);
    return _factor(i, j);
  }

  const T& L(size_t i, size_t j) const {
    assert(i > j);
    return _factor(i, j);
  }

  void Factorize(MT &f) {
    size_t dim = f.Dim();

    for (size_t c = 0; c < dim; ++c) {
      for (size_t r = 0; r <= c; ++ r) {
        for (size_t rr = 0; rr < r; ++ rr) {
	  f(r, c) -= f(r, rr) * f(rr, c);
	}
      }
      for (size_t r = c + 1; r < dim; ++r) {
        for (size_t rr = 0; rr < c; ++rr) {
	  f(r, c) -= f(r, rr) * f(rr, c);
	}
	f(r, c) /= f(c, c);
      }
    }

/*
    for (size_t c = 0; c < dim; ++c) {
      for (size_t r = 1; r <= c; ++r) {
        for (size_t rr = 0; rr < c; ++rr) {
	  f(r, c) -= f(r, rr) * f(rr, c);
    std::cout<<"f = \n"<<f;
	}
      }
      for (size_t r = c + 1; r < dim; ++r) {
        for (size_t rr = 0; rr < c; ++rr) {
	  f(r, c) -= f(r, rr) * f(rr, c);
    std::cout<<"f = \n"<<f;
	}
	f(r, c) /= f(c, c);
    std::cout<<"f = \n"<<f;
      }
    }
*/

  }
};

#endif

